#include <stddef.h>

#include "system.h"
#include "altera_avalon_pio_regs.h"
#include "alt_types.h"

#include "core.h"
#include "globals.h"
#include "keypad.h"
#include "lcd.h"
#include "pwms.h"

/* Support functions */
op_fnptr GetMovementFunction(void)
{    
    if (keyp_base_left == ENABLED)
    {
        return MoveBaseLeft;
    }
    else if (keyp_base_right == ENABLED)
    {
        return MoveBaseRight;
    }
    else if (keyp_elbow_down == ENABLED)
    {
        return MoveElbowDown;
    }
    else if (keyp_elbow_up == ENABLED)
    {
        return MoveElbowUp;
    }
    else if (keyp_shoulder_down == ENABLED)
    {
        return MoveShoulderDown;
    }
    else if (keyp_shoulder_up == ENABLED)
    {
        return MoveShoulderUp;
    }
    else if (keyp_gripper_close == ENABLED)
    {
        return InvertGripper;
    }
    else if (keyp_home == ENABLED)
    {
        return Home;
    }
    else if (keyp_test == ENABLED)
    {
        return Test;
    }
    
    return NULL;
}

op_fnptr GetHousekeepingFunction(void)
{
    if (keyp_record == ENABLED)
    {
        puts("StartRecording\n");
        return StartRecording;
    }
    else if (keyp_record_end == ENABLED)
    {
        puts("StopRecording\n");
        return StopRecording;
    }
    else if (keyp_play == ENABLED)
    {
        puts("Play\n");
        return Play;
    }
    
    return NULL;
}

/* Translate keypad flags into an executable function pointer - this will be  *
 * a messy function.                                                          */
op_fnptr GetOpFromKeypadFlags(void)
{
    op_fnptr return_value = NULL;
    
    return_value = GetMovementFunction();
    if (return_value == NULL)
    {
        return_value = GetHousekeepingFunction();
    }
    
    puts("Nothing\n");
    return return_value;
}
